#coding:utf-8
import queue
import numpy as np
import cv2
import math
import time

class Graph:
    def __init__(self, W, H, obs, wet):
        self.W = W
        self.H = H
        self.seq = np.zeros((H,W))
        self.level = -1*np.ones((H,W))
        self.path = []
        self.pathAnt = []
        self.obs = obs
        self.wet = wet
        self.heading = 0
    
    # 5  4  3
    # 6  +  2
    # 7  0  1
    def calcHeading(self, p1, p2):
        rad = math.atan2(p2[1]-p1[1], p2[0]-p1[0])
        if rad < 0:
            rad += 2*math.pi
        heading = int(np.round(rad/(math.pi/4)))
        return heading

    def run(self, start, goal):        
        self.start = start
        self.goal = goal
        self.frontier = queue.PriorityQueue()
        self.frontier.put((0,start))
        self.child = dict()
        self.came_from = dict()
        self.came_from[start] = None
        self.cost_so_far = dict()
        self.cost_so_far[start] = 0
        count = 1
        self.level[start] = 0
        while not self.frontier.empty():
            current = self.frontier.get()[1]
            if self.came_from[current]:
                father = self.came_from[current]
                self.level[current] = self.level[father] + self.cost(father, current)
            self.seq[current] = count
            if current==self.goal:
                break
            count += 1
            self.child[current] = []
            # print(current)
            for next in self.neighbors(current):
                new_cost = self.cost_so_far[current] + self.cost(current, next)
                if next not in self.cost_so_far or new_cost < self.cost_so_far[next]:
                    self.cost_so_far[next] = new_cost
                    priority = new_cost + self.heuristic(self.goal, next)
                    ## ###########运动学角度约束###########
                    #if self.came_from[current]:
                    #    self.heading = self.calcHeading(self.came_from[current], current)
                    #nei_heading = self.calcHeading(current, next)
                    #diff_heading = abs(nei_heading - self.heading)
                    #if diff_heading < 1:
                    #    diff_heading = 1
                    #priority += diff_heading*10
                    ## ###########运动学角度约束###########
                    self.level[next] = priority
                    self.frontier.put((priority,next))
                    self.came_from[next] = current
                    self.child[current].append(next)
        self.path = []
        cur = goal
        while cur:
            self.path.append(cur)
            cur = self.came_from[cur]
    def cost(self, a, b):
        d = np.sqrt((a[0]-b[0])**2+(a[1]-b[1])**2)
        #coef = 10*self.wet[a] + 10*self.wet[b]
        #d = d*coef
        return d
    def heuristic(self, a, b):
        c = abs(a[0]-b[0])+abs(a[1]-b[1])
        # c = np.sqrt((a[0]-b[0])**2+(a[1]-b[1])**2)
        c = 0
        return c
    def neighbors(self, p):
        neighbors = []
        if p[0] > 0 and self.obs[p[0]-1,p[1]]==0:
            neighbors.append((p[0]-1,p[1]))
            if p[1] > 0 and self.obs[p[0]-1,p[1]-1]==0:
                neighbors.append((p[0]-1,p[1]-1))
            if p[1] < self.W-1 and self.obs[p[0]-1,p[1]+1]==0:
                neighbors.append((p[0]-1,p[1]+1))
        if p[0] < self.H-1 and self.obs[p[0]+1,p[1]]==0:
            neighbors.append((p[0]+1,p[1]))
            if p[1] > 0 and self.obs[p[0]+1,p[1]-1]==0:
                neighbors.append((p[0]+1,p[1]-1))
            if p[1] < self.W-1 and self.obs[p[0]+1,p[1]+1]==0:
                neighbors.append((p[0]+1,p[1]+1))
        if p[1] > 0 and self.obs[p[0],p[1]-1]==0:
            neighbors.append((p[0],p[1]-1))
        if p[1] < self.W-1 and self.obs[p[0],p[1]+1]==0:
            neighbors.append((p[0],p[1]+1))
        return neighbors

    def showSeq(self):
        # count = 0
        # step = 5
        count = int(self.seq[self.goal])
        step = 1
        while True:
            M = 220*np.ones((self.H,self.W,3), np.uint8)
            for h in range(self.H):
                for w in range(self.W):
                    # if self.seq[h,w] in range(1,count+1):
                    #     M[h,w,:] = [150,150,150]
                    if self.obs[h,w]==1:
                        M[h,w,:] = [100,100,100]
                    if self.wet[h,w]>0:
                        r = 100*self.wet[h,w]
                        M[h,w,:] = [150,100+r,215]
                    
            M[self.start[0],self.start[1],:] = [80,80,255]
            M[self.goal[0],self.goal[1],:] = [50,180,50]        
            height = 401
            width = 601
            M = cv2.resize(M, (width, height), interpolation=cv2.INTER_AREA)
            for h in range(0,int(height),int(height/self.H)):
                M[h,:,:] = [255,255,255]
            for w in range(0,int(width),int(width/self.W)):
                M[:,w,:] = [255,255,255]
            if count>=self.seq[self.goal]:
                for i in range(len(self.path)-1):
                    x1 = int(self.path[i][1]*width/self.W+0.5*width/self.W)
                    y1 = int(self.path[i][0]*height/self.H+0.5*height/self.H)
                    x2 = int(self.path[i+1][1]*width/self.W+0.5*width/self.W)
                    y2 = int(self.path[i+1][0]*height/self.H+0.5*height/self.H)
                    cv2.line(M, (x1,y1), (x2,y2), [255,128,128], 2)
            #height = 1000
            #width  = 2000
            #M = cv2.resize(M, (width, height), interpolation=cv2.INTER_AREA)
            #for n in range(1,count+1):
            #    for h in range(self.H):
            #        for w in range(self.W):
            #            if self.seq[h,w]==n:
            #                #cv2.putText(M, str(int(self.wet[h,w])).center(5,' '),
            #                cv2.putText(M, str(int(self.seq[h,w])).center(3,' '),
            #                            (int(0.2*width/self.W+w*width/self.W),int(0.7*height/self.H+h*height/self.H)),
            #                            cv2.FONT_HERSHEY_PLAIN, 2.5, (0,0,0), 2)
            #M = cv2.resize(M, (800, 400), interpolation=cv2.INTER_AREA)
            #cv2.imshow('Map', M)
            #cv2.waitKey(1)
            if count==0:
                cv2.waitKey()
            if count>=self.seq[self.goal]:
                break
            count += step
            #print('count={},max={}'.format(count,self.seq[self.goal]))
        cv2.imwrite('map.jpg', M)
        cv2.imshow('Map', M)
        cv2.waitKey()

